#include "datamemoryorganisation.hpp"

DataMemoryOrganisation::DataMemoryOrganisation(Pix *pix, QObject *parent) :
    QObject(parent)
{
    this->pix = pix;
    mutex = new QMutex(QMutex::Recursive);
    bank0 = new Bank();
    bank1 = new Bank();

    workingRegister = new Register(-1, -1);
    prescaler = new Register(-2, -1);

    connect(workingRegister, SIGNAL(contentChanged(int)), this, SLOT(onContentChanged(int)),Qt::UniqueConnection);
    connect(prescaler, SIGNAL(contentChanged(int)), this, SLOT(onContentChanged(int)), Qt::UniqueConnection);

    initRegister();

    //connect(portB, SIGNAL(contentChanged(int)), this, SLOT(onContentChanged(int)));
    //connect(portA, SIGNAL(contentChanged(int)), this, SLOT(onContentChanged(int)));
}


DataMemoryOrganisation::DataMemoryOrganisation(const DataMemoryOrganisation &other)
{
    //this->actualBank = other.getActualBank();
    this->workingRegister = other.getWorkingRegister();
    this->prescaler = other.prescaler;

    this->bank0 = new Bank(*other.getBank(false));
    this->bank1 = new Bank(*other.getBank(true));
}


DataMemoryOrganisation::~DataMemoryOrganisation()
{
    delete this->mutex;
    delete this->bank0;
    delete this->bank1;
    delete this->workingRegister;
    delete this->prescaler;
    delete this->timer;
}

void DataMemoryOrganisation::switchBank()
{
    QMutexLocker locker(mutex);
    if (this->getRegister(3)->isSet(5))
	this->getRegister(3)->clearBit(5);
    else
	this->getRegister(3)->setBit(5);
}

void DataMemoryOrganisation::switchBank(bool bank)
{
    QMutexLocker locker(mutex);
    if (bank == this->getRegister(3)->isSet(5))
	return;

    locker.unlock();
    switchBank();
}

Bank *DataMemoryOrganisation::getActualBank()
{
    if(!this->getRegister(3)->isSet(5)) {
	return bank0;
    }
    return bank1;
}

Bank *DataMemoryOrganisation::getBank(bool bank) const
{
    if (bank)
	return this->bank1;
    else
	return this->bank0;
}

void DataMemoryOrganisation::initRegister()
{
    //Initiate "special" registers
    int i = 0;
    //Indirect addr.(1) 0x00
    Register *indirectAddress = new Register(i, 2);
    bank0->addRegister(i, indirectAddress);
    bank1->addRegister(i, indirectAddress);
    i++;

    //TMR0/OPTION_REG 0x01
    timer = new Register(i, 0, 255);
    bank0->addRegister(i, timer);
    bank1->addRegister(i, new Register(i, 1, 255));
    i++;

    //PCL 0x02
    Register *pcl = new Register(i, 2);
    bank0->addRegister(i, pcl);
    bank1->addRegister(i, pcl);
    i++;

    //STATUS 0x03
    Register *status = new Register(i, 2, 24);
    bank0->addRegister(i, status);
    bank1->addRegister(i, status);
    i++;

    //FSR 0x04
    Register *fsr = new Register(i, 2);
    bank0->addRegister(i, fsr);
    bank1->addRegister(i, fsr);
    i++;

    //PORTA/TRISA 0x05
    this->portA = new IORegister(this->pix->getHardwareInterface(), i, 0, 0);
    bank0->addRegister(i, this->portA);
    Register *trisA = new Register(i, 1, 31);
    bank1->addRegister(i, trisA);
    this->portA->setTris(trisA);
    i++;

    //PORTB/TRISB 0x06
    this->portB = new IORegister(this->pix->getHardwareInterface(), i, 0, 0);
    bank0->addRegister(i, this->portB);
    Register *trisB = new Register(i, 1, 255);
    bank1->addRegister(i, trisB);
    this->portB->setTris(trisB);
    i++;

    //Not used 0x07
    bank0->addRegister(i, new Register(i, 0));
    bank1->addRegister(i, new Register(i, 1));
    i++;

    //EEDATA/EECON1 0x08
    bank0->addRegister(i, new Register(i, 0));
    bank1->addRegister(i, new Register(i, 1));
    i++;

    //EEADR/EECON2(1) 0x09
    bank0->addRegister(i, new Register(i, 0));
    bank1->addRegister(i, new Register(i, 1));
    i++;

    //PCLATH 0x0A
    Register *pclath = new Register(i, 2);
    bank0->addRegister(i, pclath);
    bank1->addRegister(i, pclath);
    i++;

    //INTCON 0x0B
    Register *intcon = new Register(i, 2);
    bank0->addRegister(i, intcon);
    bank1->addRegister(i, intcon);
    i++;

    //Initiate "normal" registers start at  0x0C
    for(; i <= 0x4F; i++)
    {
	Register *tempRegister = new Register(i, 2);
	bank0->addRegister(i, tempRegister);
	bank1->addRegister(i, tempRegister);
    }

    //Connect Registersignals with on_register_changed-slot
    for(i = 1; i <= 0x4F; i++)
    {
	connect(bank0->getRegister(i), SIGNAL(contentChanged(int)), this, SLOT(onContentChanged(int)), Qt::UniqueConnection);
	connect(bank1->getRegister(i), SIGNAL(contentChanged(int)), this, SLOT(onContentChanged(int)), Qt::UniqueConnection);
    }
}

IORegister *DataMemoryOrganisation::getPortA()
{
    return portA;
}

IORegister *DataMemoryOrganisation::getPortB()
{
    return portB;
}

Register *DataMemoryOrganisation::getWorkingRegister() const
{
    return workingRegister;
}

Register *DataMemoryOrganisation::getPrescaler()
{
    return prescaler;
}


void DataMemoryOrganisation::onContentChanged(int oldValue)
{    
    Register *intcon = this->getActualBank()->getRegister(0xB);
    Register *options = this->getRegister(0x81);
    Register *reg = static_cast<Register*>(QObject::sender());
    emit registerChanged(reg);

    // Check if specific Bank has been set and switch if needed
    /* if (reg->getBank() > -1 && reg->getBank() < 2)
	switchBank(reg->getBank() == 1); */

    // Status Register
    if(reg->getAddress() == 3)
    {
	//this->actualBank = reg->isSet(5);
	emit switchBankSignal(this->getRegister(3)->isSet(5));
    }


    /********** **********
     * INTERRUPTS
     ********** **********/

    //Port A Register
    if(reg->getAddress() == 0x5)
    {
	bool portAInput = portA->isInput(4);
	int tempOldValue = oldValue & 16;
	int tempNewValue = (portA->getValue()) & 16;
	bool portA4Changed = (tempOldValue != tempNewValue);
	bool portA4Value = portA->isSet(4);
	bool t0cs = options->isSet(5);
	bool t0se = options->isSet(4);
	if(t0cs && portAInput && portA4Changed)
	{
	    int prescalerOldValue = prescaler->getValue();
	    bool psa = options->isSet(3);
	    if(t0se && !portA4Value)
	    {
		if(!psa)
		{
		    (*prescaler)--;
		}
	    }
	    else if(!t0se && portA4Value)
	    {
		if(!psa)
		{
		    (*prescaler)--;
		}
	    }
	    int prescalerNewValue = prescaler->getValue();
	    int timerOldValue = timer->getValue();
	    bool prescalerOldValue0 = (prescalerOldValue == 0);
	    bool prescalerNewValue255 = (prescalerNewValue == 255);
	    if(prescalerOldValue0 && prescalerNewValue255)
	    {
		(*timer)++;
		prescaler->reset();
	    }
	    if(timerOldValue == 255 && timer->getValue() == 0)
	    {
		intcon->setBit(2);
	    }
	    else
	    {
		intcon->clearBit(2);
	    }
	    bool gie = intcon->isSet(7);
	    bool toie = intcon->isSet(5);
	    bool toif = intcon->isSet(2);
	    bool timer0 = timer->getValue() == 0;
	    if(timer0 && gie && toie && toif)
	    {
		emit interrupt();
	    }
	}
    }
    //Port B Register
    else if(reg->getAddress() == 0x6)
    {
	int tempOldValue = oldValue & 1;
	int newValue = portB->getValue() & 1;
	if(tempOldValue != newValue)
	{
	    if(portB->isSet(0) ^ options->isSet(6))
	    {
		intcon->clearBit(1);
	    }
	    else
	    {
		intcon->setBit(1);
	    }
	}

	tempOldValue = oldValue & 0b11110000;
	newValue = portB->getValue() & 0b11110000;
	if(tempOldValue != newValue)
	{
	    intcon->setBit(0);
	}
	else
	{
	    intcon->clearBit(0);
	}

	//Global Interrupt
	if(intcon->isSet(7))
	{
	    if((intcon->isSet(4) && intcon->isSet(1)) || (intcon->isSet(3) && intcon->isSet(0)))
	    {
		emit interrupt();
	    }
	}
    }

    // OPTIONS REGISTER
    else if(reg->getAddress() == 1 && reg->getBank() == 1)
    {
	int maskedOldValue = oldValue & 0b1111;
	int maskedNewValue = options->getValue() & 0b1111;
	if(maskedOldValue != maskedNewValue)
	{
	    int value = 0;
	    if(options->isSet(3))
		value = pow(2, (options->getValue() & 0b111)) - 1;
	    else
		value = pow(2, ((options->getValue() & 0b111) + 1)) - 1;
	    prescaler->setStandard(value);
	    prescaler->reset();
	}
    }
}

void DataMemoryOrganisation::onRequestChangeRegister(int address, int value)
{
    Register *reg = this->getRegister(address);

    //Switch to the correct Bank
    /*if ((reg->getBank() == 0 && this->getRegister(3)->isSet(5)) || (reg->getBank() == 1 && !this->getRegister(3)->isSet(5)))
	this->switchBank();*/

    reg->setValue(value, true);
}


Register* DataMemoryOrganisation::getRegister(int address)
{
    if ((address & 128) > 0)
	return this->bank1->getRegister((address & 127));
    else
	return this->bank0->getRegister((address & 127));
}

void DataMemoryOrganisation::reset()
{
    //disconnect(portB, SIGNAL(contentChanged(int)), this, SLOT(onContentChanged(int)));
    //disconnect(portA, SIGNAL(contentChanged(int)), this, SLOT(onContentChanged(int)));
    this->bank1->reset();
    this->bank0->reset();
    this->workingRegister->clear();
    this->prescaler->reset();
    //connect(portA, SIGNAL(contentChanged(int)), this, SLOT(onContentChanged(int)));
    //connect(portB, SIGNAL(contentChanged(int)), this, SLOT(onContentChanged(int)));
}

void DataMemoryOrganisation::resetNormalOperation()
{
    //disconnect(portB, SIGNAL(contentChanged(int)), this, SLOT(onContentChanged(int)));
    //disconnect(portA, SIGNAL(contentChanged(int)), this, SLOT(onContentChanged(int)));
    this->getRegister(2)->reset();
    (*this->getRegister(3)) &= 0b00011111;
    this->getRegister(0xA)->reset();
    (*this->getRegister(0xB)) &= 0b00000001;
    this->getRegister(81)->reset();
    this->getRegister(85)->reset();
    this->getRegister(86)->reset();
    (*this->getRegister(88)) &= 0b00001000;
    this->prescaler->reset();
    //connect(portA, SIGNAL(contentChanged(int)), this, SLOT(onContentChanged(int)));
    //connect(portB, SIGNAL(contentChanged(int)), this, SLOT(onContentChanged(int)));
}


void DataMemoryOrganisation::onRequestSwitchBank(bool bank)
{
    if (bank)
	this->getRegister(3)->setBit(5);
    else
	this->getRegister(3)->clearBit(5);
}
